/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include "OwnQHullND.hpp"

#include <iostream>
#include <fstream>
#include <math.h>

#if defined(__cplusplus)
extern "C"
{
#endif
#include <stdio.h>
#include <stdlib.h>

#include <qhull/src/qhull.h>
#include <qhull/src/mem.h>
#include <qhull/src/qset.h>
#include <qhull/src/geom.h>
#include <qhull/src/merge.h>
#include <qhull/src/poly.h>
#include <qhull/src/io.h>
#include <qhull/src/stat.h>

#include <qhull/src/libqhull.h>
#if defined(__cplusplus)
}
#endif

#include <boost/numeric/ublas/vector.hpp>

using namespace std;
using namespace rw::geometry;
using namespace boost::numeric;
// Contact pos, normal, hastighed, depth, idA, idB

void OwnQHull::build(size_t dim,
		double *coords,
		size_t nrCoords,
		std::vector<int>& vertIdxs,
		std::vector<int>& faceIdxs,
		std::vector<double>& faceNormals,
		std::vector<double>& faceOffsets)
{
	vertIdxs.clear();
	faceIdxs.clear();
	faceNormals.clear();
	faceOffsets.clear();

	std::vector<int>& result = vertIdxs;

	int curlong, totlong; /* used !qh_NOmem */
	int exitcode;
	boolT ismalloc = false;
	int argc = 0;
	char** argv = NULL;
	qh_init_A(0, 0, stderr, argc, argv); /* sets qh OwnQHull_command */
	//qh_init_A (stdin, stdout, stderr, argc, argv);  /* sets qh OwnQHull_command */
	//char flags[] = "OwnQHull Qt Pp Qs";

	//char flags[] = "OwnQHull Pp n Qt Qx QJ C-0.0001";
	//char flags[] = "OwnQHull Pp Qs QJ C-0.0001 n";
	//char flags[] = "OwnQHull Qx Qs W1e-1 C1e-2 Qt Pp n"; //graspit
	//char flags[] = "OwnQHull Qs Pp Qt n";

	char flags[] = "qhull QJ QR-2";


	exitcode = qh_new_qhull(dim, nrCoords, coords, ismalloc, flags, NULL, stderr);

	if (!exitcode) {
		//        facetT *facet;
		ublas::vector<double> center = ublas::zero_vector<double>(dim);
		ublas::vector<double> vert = ublas::zero_vector<double>(dim);
		size_t nrVerts = 0;
		vertexT *vertex;//, **vertexp;
		FORALLvertices {
			int vertexIdx = qh_pointid(vertex->point);
			result.push_back(vertexIdx);

			for(size_t i=0; i<dim; i++){
				double val = vertex->point[i];
				center[i] = center[i] + val;
			}
			nrVerts++;
			//result->push_back(contacts->at(vertexIdx));
			//contacts->at(vertexIdx)=NULL;
		}
		center /= (double)nrVerts;

		// also find all facets, such that we can recreate the hull
		ublas::vector<double> zerov = ublas::zero_vector<double>(dim);
		facetT *facet;
		FORALLfacets {
			/*
            std::cout << "CENTER: ";
            for(int i=0;i<dim;i++){
                std::cout << facet->center[i] << ", ";
            }
            std::cout << std::endl;
			 */
			//std::cout << facet->vertices->maxsize << std::endl;
			int vertex_n, vertex_i;
			//std::cout << "{ ";
			// if offset is positive then the center is outside
			//ublas::vector<double> n = ublas::zero_vector<double>(dim);
			//ublas::vector<double> v = ublas::zero_vector<double>(dim);
			for(size_t j=0;j<dim;j++){
				faceNormals.push_back( facet->normal[j] );
			}
			faceOffsets.push_back(facet->offset);

			//int idx = 0;
			FOREACHvertex_i_(facet->vertices){
				int vertexIdx = qh_pointid(vertex->point);
				faceIdxs.push_back(vertexIdx);
				//idx = vertexIdx;
				//std::cout << vertexIdx << " ";
			}
			//std::cout << idx << std::endl;
			//for(size_t j=0;j<dim;j++){
			//    v[j] = coords[dim*idx+j];
			//}
			//n = n/norm_2(n);

			//double dist;
			//qh_distplane(&zerov[0],facet,&dist);
			//if( dist>0 ){
			//    std::cout << "GRASP IS NOT FORCE CLOSURE" << std::endl;
			//}


			//std::cout << " }\n";
		}
		//myfile << nrCoords << " " << nrVerts << " " << faceOffsets.size() << std::endl;
		/*for (size_t i = 0; i < normalsSize; i++) {
						myfile << projNormals[i][0] << " ";
						myfile << projNormals[i][1] << " ";
						myfile << projNormals[i][2] << " ";
						myfile << projNormals[i][3] << " ";
						myfile << std::endl;
					}*/
		//myfile.close();


	} else {
		cout<<"Not Exit Code!";
	}
	if (qh VERIFYoutput && !qh FORCEoutput && !qh STOPpoint && !qh STOPcone)
		qh_check_points();

	qh_freeqhull( False );
	qh_memfreeshort (&curlong, &totlong);
	if (curlong || totlong)
		fprintf (stderr, "OwnQHull internal warning (main): did not free %d bytes of long memory (%d pieces)\n",totlong, curlong);
	//delete[] coords;

}

void OwnQHull::build_subspace(size_t dim,
		double *coords,
		size_t nrCoords,
		std::vector<rw::math::VectorND<3> >& returnVertices,
		std::vector<rw::math::VectorND<3> >& returnFaces,
		//std::vector<int>& vertIdxs,
		//std::vector<int>& faceIdxs,
		std::vector<double>& faceNormals,
		std::vector<double>& faceOffsets,
		const rw::math::VectorND<6> origo)
{
	//vertIdxs.clear();
	returnVertices.clear();
	returnFaces.clear();
	//faceIdxs.clear();
	faceNormals.clear();
	faceOffsets.clear();

	//std::vector<int>& result = vertIdxs;

	int curlong, totlong; /* used !qh_NOmem */
	int exitcode;
	boolT ismalloc = false;
	int argc = 0;
	char** argv = NULL;
	qh_init_A(0, 0, stderr, argc, argv);
	char flags[] = "qhull QJ QR-2";

	exitcode = qh_new_qhull(dim, nrCoords, coords, ismalloc, flags, NULL, stderr);

	/*std::ofstream myfile;
	myfile.open ("debug.txt", std::ios::app);
	myfile << "-------------" << std::endl;
	for (size_t i = 0; i < nrCoords/dim; i++) {
		myfile << coords[i*dim+0] << " ";
		myfile << coords[i*dim+1] << " ";
		myfile << coords[i*dim+2] << " ";
		myfile << coords[i*dim+3] << " ";
		myfile << coords[i*dim+4] << " ";
		myfile << coords[i*dim+5] << " ";
		myfile << std::endl;
	}
	myfile.close();*/

	if (!exitcode) {
		ublas::vector<double> center = ublas::zero_vector<double>(dim);
		ublas::vector<double> vert = ublas::zero_vector<double>(dim);
		size_t nrVerts = 0;
		vertexT *vertex;//, **vertexp;
		FORALLvertices {
			/*rw::math::VectorND<3> vec;
			for (size_t i=0; i < 3; i++) {
				vec[i] = vertex->point[i];
			}
			returnVertices.push_back(vec);*/

			for(size_t i=0; i<dim; i++){
				double val = vertex->point[i];
				center[i] = center[i] + val;
			}
			nrVerts++;
		}
		center /= (double)nrVerts;

		// also find all facets, such that we can recreate the hull
		ublas::vector<double> zerov = ublas::zero_vector<double>(dim);
		facetT *facet;
		FORALLfacets {
			for(size_t j=0;j<dim;j++){
				faceNormals.push_back( facet->normal[j] );
			}
			faceOffsets.push_back(facet->offset);

			/*int vertex_n, vertex_i;
			FOREACHvertex_i_(facet->vertices){
				int vertexIdx = qh_pointid(vertex->point);
				faceIdxs.push_back(vertexIdx);
			}*/
		}

		int normalsSize = faceNormals.size()/dim;
		double** projNormals = (double **) malloc(normalsSize*sizeof(double*));
		int k = 0;
		for (int i = 0; i < normalsSize; i++) {
			double len = sqrt(faceNormals.at(i*dim)*faceNormals.at(i*dim) +
					faceNormals.at(i*dim+1)*faceNormals.at(i*dim+1) +
					faceNormals.at(i*dim+2)*faceNormals.at(i*dim+2));
			if (len > 1e-9) {
				projNormals[k] = (double*) malloc(4*sizeof(double));
				projNormals[k][0] = faceNormals.at(i*dim)/len;
				projNormals[k][1] = faceNormals.at(i*dim+1)/len;
				projNormals[k][2] = faceNormals.at(i*dim+2)/len;
				projNormals[k][3] = (faceOffsets.at(i) +
						faceNormals.at(i*dim+3)*origo[3] +
						faceNormals.at(i*dim+4)*origo[4] +
						faceNormals.at(i*dim+5)*origo[5])/len;
				k++;
			}
		}

		if (qh VERIFYoutput && !qh FORCEoutput && !qh STOPpoint && !qh STOPcone)
			qh_check_points();

		qh_freeqhull( False );
		qh_memfreeshort (&curlong, &totlong);
		if (curlong || totlong)
			fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n",totlong, curlong);

		coordT *array = new coordT[k*3];
		coordT *p = &array[0];
		qh_init_A(0, 0, stderr, argc, argv);

		char options[200];
		sprintf(options, "qhull -H0,0,0 -QJ");
		qh_initflags(options);
		qh_setfeasible(3);

		for (int i = 0; i < k; i++) {
			qh_sethalfspace(3, p, &p, projNormals[i], &(projNormals[i][3]), qh feasible_point);
		}
		qh_init_B(&array[0], k, 3, ismalloc);
		qh_qhull();
		qh_check_output();

		std::vector<double> newVertices;
		dim = 3;


		zerov = ublas::zero_vector<double>(dim);
		FORALLfacets {
			coordT *feasiblep= qh feasible_point;
			rw::math::VectorND<3> vec;
			for(size_t j=0;j<dim;j++){
				newVertices.push_back( (facet->normal[j]/-facet->offset) + (*feasiblep));
				vec[j] = (facet->normal[j]/-facet->offset) + *feasiblep;
			}
			returnVertices.push_back(vec);
		}
		double *newVerticesArray = new double[newVertices.size()];
		for (size_t i = 0; i < newVertices.size(); i++) {
			newVerticesArray[i] = newVertices.at(i);
		}

		//
		// use adjacency information to build faces of the volume
		//

		/*double dot;
		rw::math::Vector3D<> testNormal, refNormal;

		int numfacets, numsimplicial, numridges, totneighbors, numneighbors, numcoplanars;
		setT *vertices, *vertex_points, *coplanar_points;
		int numpoints= qh num_points + qh_setsize (qh other_points);
		int vertex_i, vertex_n;
		facetT *neighbor, **neighborp;

		qh_countfacets (qh facet_list, NULL, !qh_ALL, &numfacets, &numsimplicial,
				&totneighbors, &numridges, &numcoplanars);

		qh_vertexneighbors();
		vertices= qh_facetvertices (qh facet_list, NULL, !qh_ALL);
		vertex_points= qh_settemp (numpoints);
		coplanar_points= qh_settemp (numpoints);
		qh_setzero (vertex_points, 0, numpoints);
		qh_setzero (coplanar_points, 0, numpoints);
		FOREACHvertex_(vertices)
		qh_point_add (vertex_points, vertex->point, vertex);
		FORALLfacet_(qh facet_list) {
			FOREACHpoint_(facet->coplanarset)
		    				  qh_point_add (coplanar_points, point, facet);
		}

		FOREACHvertex_i_(vertex_points) {
			if (vertex) {
				numneighbors= qh_setsize (vertex->neighbors);

				numInLoop = numneighbors;
				qh_order_vertexneighbors (vertex);
				j=0;
				FOREACHneighbor_(vertex) {
					if (!neighbor->visitid) {
						fprintf(stderr,"Uh oh! neighbor->visitId==0, -neighbor->id: %d\n",-neighbor->id);
						numInLoop--;
					}
					else
						indices[j] = neighbor->visitid - 1;

					if (j>0 && ((hullCoords[indices[j]]-hullCoords[indices[j-1]]).len() < IVTOL ||
							(hullCoords[indices[j]]-hullCoords[indices[0]]).len() < IVTOL))
					{
						numInLoop--;
					}
					else j++;
				}
			} else if ((facet= SETelemt_(coplanar_points, vertex_i, facetT))) {
				numInLoop=1;
			} else {
				numInLoop=0;
				continue;
			}

			if (numInLoop<3) {
				DBGP("too few vertices to make a face. Ignoring ...");
				continue;
			}

			// check if the current orientation of the face matches the plane's normal
			testNormal = (hullCoords[indices[1]] - hullCoords[indices[0]]) * (hullCoords[indices[j-1]] - hullCoords[indices[0]]);
			refNormal = vec3(planes[vertex_i][0],planes[vertex_i][1],planes[vertex_i][2]);

			if ((dot = testNormal % refNormal) > 0.0) {
				for (j=0;j<numInLoop;j++)
					hullIndices.push_back(indices[j]);
				hullIndices.push_back(-1);
			} else {
				// reverse the vertex ordering
				for (j=numInLoop-1;j>=0;j--)
					hullIndices.push_back(indices[j]);
				hullIndices.push_back(-1);
			}
		}*/










		/*
		// also find all facets, such that we can recreate the hull
		zerov = ublas::zero_vector<double>(dim);
		FORALLfacets {
			for(size_t j=0;j<dim;j++){
				newVertices.push_back( (facet->normal[j]/-facet->offset) );
			}
		}

		double *newVerticesArray = new double[newVertices.size()];
		for (size_t i = 0; i < newVertices.size(); i++) {
			newVerticesArray[i] = newVertices.at(i);
		}

		qh_freeqhull( False );
		qh_memfreeshort (&curlong, &totlong);

		char flags2[] = "qhull QJ QR-2";
		qh_init_A(0, 0, stderr, argc, argv);
		exitcode = qh_new_qhull(dim, newVertices.size()/dim, newVerticesArray, ismalloc, flags2, NULL, stderr);

		//vertIdxs.clear();
		returnVertices.clear();
		returnFaces.clear();
		//faceIdxs.clear();
		faceNormals.clear();
		faceOffsets.clear();

		if (!exitcode) {
			center = ublas::zero_vector<double>(dim);
			vert = ublas::zero_vector<double>(dim);
			FORALLvertices {
				rw::math::VectorND<3> vec;
				for(size_t i=0; i<dim; i++){
					vec[i] = vertex->point[i];
				}
				returnVertices.push_back(vec);
			}

			// also find all facets, such that we can recreate the hull
			zerov = ublas::zero_vector<double>(dim);
			FORALLfacets {
				for(size_t j=0;j<dim;j++){
					faceNormals.push_back( (facet->normal[j]) );
				}
				faceOffsets.push_back(facet->offset);

				int vertex_n, vertex_i;
				FOREACHvertex_i_(facet->vertices){
					rw::math::VectorND<3> vec;
					for(size_t j=0; j<dim; j++){
						vec[j] = vertex->point[j];
					}
					returnFaces.push_back(vec);
				}
			}

			std::ofstream myfile;
			myfile.open ("debug.txt", std::ios::app);
			myfile << "-------------" << std::endl;
			myfile << nrCoords << " " << nrVerts << " " << normalsSize << " " << k << " " << newVertices.size()/dim << " " << returnVertices.size() << " " << faceOffsets.size() << std::endl;
			if (nrCoords == 55) {
				for (size_t i = 0; i < returnVertices.size(); i++) {
					//myfile << returnVertices[i][0] << " " << returnVertices[i][1] << " " << returnVertices[i][2] << " " ;
				}
				for (size_t i = 0; i < returnFaces.size(); i++) {
					//myfile << returnFaces[i][0] << " " << returnFaces[i][1] << " " << returnFaces[i][2] << " " ;
					myfile << faceOffsets[i] << " ";
				}
				for (size_t i = 0; i < newVertices.size(); i++) {
					//myfile << newVertices[i] << " ";
				}
				myfile << std::endl;
			}
			myfile.close();

			delete [] array;
			for (int i = 0; i < k; i++) {
				free(projNormals[i]);
			}
			free(projNormals);
		}*/


		qh_freeqhull( False );
		qh_memfreeshort (&curlong, &totlong);

	} else {
		cout<<"Not Exit Code!";
	}
	if (qh VERIFYoutput && !qh FORCEoutput && !qh STOPpoint && !qh STOPcone)
		qh_check_points();

	qh_freeqhull( False );
	qh_memfreeshort (&curlong, &totlong);
	if (curlong || totlong)
		fprintf (stderr, "OwnQHull internal warning (main): did not free %d bytes of long memory (%d pieces)\n",totlong, curlong);

}
